package org.robotfish.geometry;

import java.io.StringWriter;
import org.ejml.simple.SimpleMatrix;
import org.robotfish.utils.SVGDocument;
import org.w3c.dom.Comment;
import org.w3c.dom.Element;


/**
 * Base Class for transformations. Z coordinates always stay the same. X and Y coordinates linear combination of 2 arc transforms.
 * @author Mart Anton
 * @see <a href="http://code.google.com/p/fishrobot/wiki/TransformationExplained">Transformation Explained</a>
 */
public abstract class Transformation implements Comparable<Transformation> {
	
	/**
	 * The start of the interval of reference coordinates
	 */
	double coordinateValue;
	
/*	public static Transformation create3DTransformation(SimpleMatrix nose, double alpha[], TransformationSettings settings) {
		SegmentedTransformation levelZ = new SegmentedTransformation(2);
		
		
		
		settings.joints[1].stretchRegion = 1;
		
		return null;
	}*/
	
	
	public double getCoordinateValue() {
		return coordinateValue;
	}

	@Override
	public int compareTo(Transformation arg0) {
		if (arg0.coordinateValue> coordinateValue)
			return 1;
		if (arg0.coordinateValue< coordinateValue)
			return -1;
		return 0;
	}

	/**
	 * Find new coordinates for the point 
	 * @param point teisendatav punkt
	 * @return uus punkt
	 */
	public abstract SimpleMatrix transform(SimpleMatrix point);
	
	/**
	 * Interpolates the results of two transformations 
	 * @param point
	 * @param nextInterval
	 * @param m0 relative distance of the point from the start of this interval. m0 in [0,1) 
	 * @return
	 */
	public SimpleMatrix interpolate(SimpleMatrix point, Transformation nextInterval, double m0) {
		SimpleMatrix result;
		SimpleMatrix result0;
		SimpleMatrix result1;
		result0 = transform(point); 
		result1 = nextInterval.transform(point);
		result = result0.scale(m0).plus(result1.scale(1-m0));
		
		SimpleMatrix trnPoint = point.copy();
		trnPoint.set(0, result.get(0));
		trnPoint.set(1, result.get(1));
		return trnPoint;
	}
	
	/**
	 * Gives the type of coordinate this transformation is associated with
	 * @return 0 is x, 1 is y and 2 is z
	 */
	public abstract int getCoordinate();
	
	/**
	 * Add path, text and comment revealing information about this transformation
	 * @param doc SCV document http://www.w3.org/TR/SVG11/
	 * @param parent parent node
	 * @param arcLength length of the arc (next.start-this.start if next arc exists)
	 */
	public void appendSVGElements(SVGDocument doc, Element parent, double arcLength) {
		Comment comment = doc.createComment(getClass().getName()+".appendSVGElements: coordinate="+getCoordinate()+", start="+coordinateValue);
        parent.appendChild(comment);
	}
	
	public SVGDocument toSVGDocument() {
		SVGDocument doc = new SVGDocument(); 
		
		//create content recursively
		appendSVGElements(doc, doc.getRoot(), 0);
		
		return doc;
	}
	
	/**
	 * Writes out a SVG file representing this transformation
	 */
	public String toString() {
        StringWriter sw = new StringWriter();
		
		/////////////////
		//Output the XML
		toSVGDocument().write(sw);
        return sw.toString();
	}

	/**
	 * Get Ray at the beginning of the transformation 
	 */
	public abstract SimpleMatrix getRay();

	/**
	 * http://en.wikipedia.org/wiki/Sinc_function 
	 * @param x
	 * @return sin(x)/x
	 */
	public static double sinc(double x)
	{
		double x2;
		double x4;
		if (Math.abs(x)<0.05) { // |x|<0.05 accuracy 1E-16
			x2 = x*x;
			x4 = x2*x2;
			return 1-(1.0/6.0)*x2+(1.0/120.0)*x4-(1.0/5040.0)*x4*x2;
		} else {
			return Math.sin(x)/x;
		}
	}
	
	/**
	 * @param tangentVector
	 * @param referenceAngle
	 * @return atan2(tangent.X, tangent.Y) in [referenceAngle-Math.PI, referenceAngle+Math.PI] 
	 */
	public static double getAngle(SimpleMatrix tangentVector, double referenceAngle) {
		double a = Math.atan2(tangentVector.get(1), tangentVector.get(0));
		// TODO a tuleb viia lõikku [referenceAngle-Math.PI, referenceAngle+Math.PI]
		a += Math.PI - referenceAngle ;
		a %= (2*Math.PI);
		a += referenceAngle - Math.PI; 
		return a;
	}
	
	
	/**
	 * create tangential vector {cos(a), sin(a), {0}}
	 * @param a angle
	 * @return unit vector with direction a
	 */
	public static SimpleMatrix tangentVector(double a) {
		return new SimpleMatrix(new double[][] {{Math.cos(a)}, {Math.sin(a)}, {0}});
//		return new SimpleMatrix(new double[][] {{Math.cos(a), Math.sin(a)}});
	}
	
	/**
	 * create normal vector 
	 * @param a angle
	 * @return unit vector with direction a+90deg
	 */
	public static SimpleMatrix normalVector(double a) {
		return new SimpleMatrix(new double[][] {{-Math.sin(a)}, {Math.cos(a)}, {0}});
	}

}

